/************************************************************
 * M3D_DUALQUAT.CPP                                19.11.2006
 *
 * Dual quaternions support
 ************************************************************/

#include <m3d/m3d_stdafx.h>

#include <ap_comm.h>
#include <m3d.h>

/*************************************************************
* m3dQUAT_DUAL::m3dQUAT_DUAL()
* Construct quaternion from rotation quaternion and translation vector
*************************************************************/
m3dQUAT_DUAL::m3dQUAT_DUAL (const m3dQUAT * __RESTRICT qrot, const m3dV * __RESTRICT trans)
{
   q0 = *qrot;

   qe.s = 0;
   m3dScaleVector(trans, 0.5, &qe.v);

   m3dMultiplyQuat(&qe, &q0, &qe);
}

/*************************************************************
* m3dQUAT_DUAL::Inverse()
* Inversion of a dual quaternion with non-zero nondual part
*************************************************************/
void m3dQUAT_DUAL::Inverse (void)
{
   m3dQUAT_DUAL inv;
   float normSq0 = m3dInnerProductQuat(&q0, &q0);
   float normSqE = 2 * m3dInnerProductQuat(&q0, &qe);

   ASSERT(!m3dIsZero(normSq0));

   float invNormSq0 = 1 / normSq0;
   float invNormSqE = - normSqE / (normSq0 * normSq0);

   m3dQUAT_DUAL conj;
   Conjugate(&conj);

   m3dScaleQuat(&conj.q0, invNormSq0, &inv.q0);

   m3dScaleQuat(&conj.qe, invNormSq0, &inv.qe);
   m3dQUAT temp;
   m3dScaleQuat(&conj.q0, invNormSqE, &temp);
   m3dAddQuat(&conj.qe, &temp, &conj.qe);

   *this = inv;
}

/*************************************************************
* m3dQUAT_DUAL::Conjugate()
*************************************************************/
void m3dQUAT_DUAL::Conjugate (void)
{
   _m3dScaleVector(&q0.v, -1.f);
   _m3dScaleVector(&qe.v, -1.f);
}

/*************************************************************
* m3dQUAT_DUAL::RotationNormalize()
* normalization of the rotation component of dual quaternion 
*     (divide both nondual and dual part by magnitude of nondual part)
*************************************************************/
void m3dQUAT_DUAL::RotationNormalize (void)
{
   float magn = q0.GetNorm();

   ASSERT(!m3dIsZero(magn));
   m3dScaleQuat(&q0, 1 / magn, &q0);
   m3dScaleQuat(&qe, 1 / magn, &qe);
}

/*************************************************************
* m3dQUAT_DUAL::PluckerNormalize()
* normalization of the translation component of dual quaternion 
*     (project dual part so that it satisfies Plucker's condition)
*************************************************************/
void m3dQUAT_DUAL::PluckerNormalize (void)
{
   float len = m3dInnerProductQuat(&q0, &q0);
   ASSERT(!m3dIsZero(len));

   m3dQUAT temp;
   m3dScaleQuat(&q0, m3dInnerProductQuat(&q0, &qe) / len, &temp);
   m3dSubtractQuat(&qe, &temp, &qe);
}

/*************************************************************
* m3dQUAT_DUAL::Normalize()
* Normalize quaternion
*************************************************************/
void m3dQUAT_DUAL::Normalize (void)
{
   // NOTE: the order of normalizations does not matter
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   vec_float4 q0v = vec_load_unaligned((float*)&q0);
   vec_float4 qev = vec_load_unaligned((float*)&qe);

   vec_float4 q0len = vec_dot4(q0v, q0v);

   // inverse magnitutde
   vec_float4 q0imagn = rsqrtf4(q0len); //vec_rsqrte(q0len); <-- Too inaccurate.

   q0v = vec_madd(q0v, q0imagn, (vec_float4){0.0f});     // m3dScaleQuat
   qev = vec_madd(qev, q0imagn, (vec_float4){0.0f});

   vec_float4 q0ilen = vec_re(q0len);  // 1 / len
   
   vec_float4 tmp = vec_madd(q0v, vec_madd(vec_dot4(q0v, qev), q0ilen, (vec_float4){0.0f}), (vec_float4){0.0f});

   qev = vec_sub(qev, tmp);

   vec_store_unaligned(q0v, (float*)&q0);
   vec_store_unaligned(qev, (float*)&qe);
#else
   RotationNormalize();
   PluckerNormalize();
#endif
}

/*************************************************************
* m3dQUAT_DUAL::Transform()
* 
*************************************************************/
void m3dQUAT_DUAL::Transform (const m3dV * __RESTRICT a, m3dV * __RESTRICT res) const
{
   m3dQUAT_DUAL correctTrans;
   float scale = ExtractScale(&correctTrans);

   m3dQUAT_DUAL qd;
   qd.q0.Identity();
   qd.qe.s = 0;

   m3dScaleVector(a, scale, &qd.qe.v);

   m3dQUAT_DUAL qd1;
   m3dMultiplyQuatDual(&correctTrans, &qd, &qd1);

   m3dQUAT_DUAL thisConj;
   correctTrans.Conjugate(&thisConj);
   m3dScaleQuat(&thisConj.qe, -1, &thisConj.qe);

   m3dMultiplyQuatDual(&qd1, &thisConj, &qd1);

   *res = qd1.qe.v;
}

/*************************************************************
* m3dQUAT_DUAL::Identity()
* 
*************************************************************/
void m3dQUAT_DUAL::Identity (void)
{
   q0.Identity();
   qe.s = 0;
   m3dMakeVector(&qe.v, 0, 0, 0);
}

/*************************************************************
* m3dQUAT_DUAL::SetZero()
*************************************************************/
void m3dQUAT_DUAL::SetZero (void)
{
   q0.s = 0;
   q0.v.x = q0.v.y = q0.v.z = 0;
   qe.s = 0;
   qe.v.x = qe.v.y = qe.v.z = 0;
}

/*************************************************************
* m3dQUAT_DUAL::FromRTMatrix()
* Conversion from rigid transformation matrix to dual quaternion
* RET: scale part of transformation
*************************************************************/
float m3dQUAT_DUAL::FromRTMatrix   (const m3dMATR * __RESTRICT matr)
{
   float transformScale;
#if defined _AP_PS3 && !defined M3D_SUPPRESS_OPTIMIZATION
   // Flatten and inline all these function calls to avoid
   // many unnecessary loads, stores, and LHS.

   // Constants
   const vec_float4 zero = (vec_float4){0.0f,0.0f,0.0f,0.0f};
   const vec_float4 one  = (vec_float4){1.0f,1.0f,1.0f,1.0f};
   const vec_float4 half = (vec_float4){0.5f,0.5f,0.5f,0.5f};

   const vec_uint4 SELECT_X = _O_VECTORMATH_MASK_0xF000;
   const vec_uint4 SELECT_Y = _O_VECTORMATH_MASK_0x0F00;
   const vec_uint4 SELECT_Z = _O_VECTORMATH_MASK_0x00F0;
   const vec_uint4 SELECT_W = _O_VECTORMATH_MASK_0x000F;

   // TODO: Look at the vec_perms and try and reduce the
   // number of constants used here.
   const vec_uchar16 PERM_XYCX = _O_VECTORMATH_PERM_XYCX;
   const vec_uchar16 PERM_YCXY = _O_VECTORMATH_PERM_YCXY;
   const vec_uchar16 PERM_CXYC = _O_VECTORMATH_PERM_CXYC;
   const vec_uchar16 PERM_ZXBX = _O_VECTORMATH_PERM_ZXBX;
   const vec_uchar16 PERM_YZAX = _O_VECTORMATH_PERM_YZAX;
   const vec_uchar16 PERM_XZYA = _O_VECTORMATH_PERM_XZYA;
   const vec_uchar16 PERM_ZXXB = _O_VECTORMATH_PERM_ZXXB;
   const vec_uchar16 PERM_YXXC = _O_VECTORMATH_PERM_YXXC;

   // Matrix.
   const vec_float4 mx = vec_load_unaligned(matr->elements,  0);
   const vec_float4 my = vec_load_unaligned(matr->elements, 16);
   const vec_float4 mz = vec_load_unaligned(matr->elements, 32);
   const vec_float4 mt = vec_load_unaligned(matr->elements, 48);

   // Get inverse scale
   const vec_float4 isx = rsqrtf4(vec_dot3(mx, mx));
   const vec_float4 isy = rsqrtf4(vec_dot3(my, my));
   const vec_float4 isz = rsqrtf4(vec_dot3(mz, mz));

   // Rotation matrix
   const vec_float4 r0 = vec_madd(mx, isx, zero);
   const vec_float4 r1 = vec_madd(my, isy, zero);
   const vec_float4 r2 = vec_madd(mz, isz, zero);
   
   // Compute q0 from rotation matrix.
   // Equivalent to:
   // q0.SetMatr(matr);


   // 4 cases
   // trace > 0
   // i == 0
   // i == 1
   // i == 2

   const vec_float4 _00_11 = vec_sel(r0, r1, SELECT_Y);
   const vec_float4 _00_11_22_00 = vec_perm(_00_11, r2, PERM_XYCX);
   const vec_float4 _11_22_00_11 = vec_perm(_00_11, r2, PERM_YCXY);
   const vec_float4 _22_00_11_22 = vec_perm(_00_11, r2, PERM_CXYC);
    
   const vec_float4 dsum = vec_add(_00_11_22_00,
                           vec_add(_11_22_00_11,
                                   _22_00_11_22));
   const vec_float4 ddif = vec_sub(_00_11_22_00,
                           vec_add(_11_22_00_11,
                                   _22_00_11_22));

   const vec_float4 traces = vec_add(vec_sel(ddif, dsum, SELECT_W), one);
   
   const vec_float4 isqrt = rsqrtf4(traces); // vec_rsqrte(traces); <-- Too inaccurate.
   const vec_float4 scale = vec_madd(isqrt, half, zero);

   const vec_float4 _12_20_01 = vec_perm(vec_sel(r1, r2, SELECT_X),
                                         r0, PERM_ZXBX);
    
   const vec_float4 _21_02_10 = vec_perm(vec_sel(r0, r2, SELECT_Y),
                                         r1, PERM_YZAX);

    
   const vec_float4 sum = vec_add(_12_20_01, _21_02_10);
   const vec_float4 dif = vec_sub(_12_20_01, _21_02_10);

   // i == 0 j == 1 k == 2
   // res0.x = ...
   // res0.y = 01 + 10
   // res0.z = 02 + 20
   // res0.w = 12 - 21
   vec_float4 res0 = vec_perm(sum, dif, PERM_XZYA);
   // res0.x = trace 
   res0 = vec_sel(res0, traces, SELECT_X);
   // res0.x = trace * (trace ^ -0.5) * 0.5 = sqrt(trace) * 0.5;
   // res0.y = (01 + 10) * s...
   res0 = vec_madd(res0, vec_splat(scale, 0), zero);

   // i == 1 j == 2 k == 0
   // res1.x = 10 + 01
   // res1.y = ...
   // res1.z = 12 + 21
   // res1.w = 20 - 02
   vec_float4 res1 = vec_perm(sum, dif, PERM_ZXXB);
   // res1.y = trace
   res1 = vec_sel(res1, traces, SELECT_Y);
   res1 = vec_madd(res1, vec_splat(scale, 1), zero);

   // i == 2 j == 0 k == 1
   // res2.x = 20 + 02
   // res2.y = 21 + 12
   // res2.z = ...
   // res2.w = 01 - 10
   vec_float4 res2 = vec_perm(sum, dif, PERM_YXXC);
   res2 = vec_sel(res2, traces, SELECT_Z);
   res2 = vec_madd(res2, vec_splat(scale, 2), zero);

   // trace > 0
   // res.x = 12 - 21
   // res.y = 20 - 02
   // res.z = 01 - 10
   // res.w = ...
   vec_float4 res3 = vec_sel(dif, traces, SELECT_W);
   res3 = vec_madd(res3, vec_splat(scale, 3), zero);

   // Now select the correct answer

   const vec_float4 _00 = vec_splat(r0, 0);
   const vec_float4 _11 = vec_splat(r1, 1);
   const vec_float4 _22 = vec_splat(r2, 2);
    
   vec_float4 q0v = vec_sel(res0, res1, vec_cmpgt(_11, _00));
   q0v = vec_sel(q0v, res2,
                 vec_and(vec_cmpgt(_22, _11), vec_cmpgt(_22, _00)));
   q0v = vec_sel(q0v, res3, vec_cmpgt(vec_splat(traces, 3), one));

   const vec_float4 q0v_store = vec_sld(q0v, q0v, 12);

   vec_st(q0v_store, 0, (float*)&q0);

   /////////

   // matr->GetScaleX(&scale)
   const vec_float4 matr_scale = vec_splat(vec_madd(isx, vec_dot3(mx, mx), zero), 0);
   vec_ste(matr_scale, 0, &transformScale);

   ///////
   // Now calculate qe

   // m3dScaleVector(&trans, 0.5f, &qe.v);
   vec_float4 qev = vec_madd(mt, half, zero);

   const vec_float4 SIGN = ((vec_float4)(vec_uint4){0x80000000,0x80000000,0x80000000,0});

   // -m3dDotProduct(&qe.v, &q0.v)
   const vec_float4 neg_qe_dot_q0 = vec_splat(vec_xor(vec_dot3(qev, q0v), SIGN), 0);

   // m3dCrossProduct(&qe.v, &q0.v, &crs);
   const vec_float4 crs = vec_cross3(qev, q0v);
   // m3dScaleVector(&qe.v, q0.s, &sv);
   const vec_float4 sv = vec_madd(qev, vec_splat(q0v, 3), zero);

   // m3dAddVector(&crs, &sv);
   qev = vec_add(crs, sv);

   // Put q.s in
   qev = vec_sel(qev, neg_qe_dot_q0, SELECT_W);

   // Put q.s in the correct place.
   const vec_float4 qev_store = vec_sld(qev, qev, 12);

   vec_st(qev_store, 0, (float*)&qe);
#else
   matr->GetScaleX(&transformScale);
   
   q0.SetMatr(matr);

   m3dV trans;
   matr->GetTranslate(&trans);

   m3dScaleVector(&trans, 0.5f, &qe.v);
   
   qe.s = - m3dDotProduct(&qe.v, &q0.v);

   m3dV crs, sv;

   m3dCrossProduct (&qe.v, &q0.v, &crs);
   m3dScaleVector  (&qe.v, q0.s, &sv);
   m3dAddVector    (&crs, &sv, &qe.v);
#endif
   return transformScale;
}

/*************************************************************
* m3dQUAT_DUAL::ToRTMatrix()
* Conversion dual quaternion to rigid transformation matrix
*************************************************************/
void m3dQUAT_DUAL::ToRTMatrix (m3dMATR * __RESTRICT matr) const
{
   // NOTE: nondual part must not be zero (but it doesn't have to be normalized)
   // The dual quaternion also needs not satisfy Plucker's relation
   q0.GetMatr(matr);

   m3dQUAT q0z;
   q0z.s = 2 * q0.s;
   m3dScaleVector(&q0.v, -2, &q0z.v);

   m3dQUAT res;
   m3dMultiplyQuat(&qe, &q0z, &res);
   q0z = res;
   matr->Translate(&q0z.v, M3D_REPLACE);
}

/*************************************************************
* m3dQUAT_DUAL::DotRealPart()
* RET : non-dual part of dot product.
*************************************************************/
float m3dQUAT_DUAL::DotRealPart    (const m3dQUAT_DUAL * __RESTRICT dq) const
{
   return m3dInnerProductQuat(&q0, &dq->q0);
}

/*************************************************************
* m3dQUAT_DUAL::DotDualPart()
* RET : dual part of dot product.
*************************************************************/
float m3dQUAT_DUAL::DotDualPart    (const m3dQUAT_DUAL * __RESTRICT dq) const
{
   return m3dInnerProductQuat(&q0, &dq->qe) + m3dInnerProductQuat(&qe, &dq->q0);
}

/*************************************************************
* m3dQUAT_DUAL::ExtractScale()
* 
* RET : transformation scale 
* IN /OUT : res - correct normalized dual quaternion
*************************************************************/
float m3dQUAT_DUAL::ExtractScale (m3dQUAT_DUAL * __RESTRICT res) const
{
   res->qe = qe;
   float scale = q0.GetNorm();
   m3dScaleQuat(&q0, 1 / scale, &res->q0);
   return scale;
}

/*************************************************************
* m3dQUAT_DUAL::StoreScale()
*************************************************************/
void m3dQUAT_DUAL::StoreScale (float scale)
{
   m3dScaleQuat(&q0, scale, &q0);
}


// 
/*************************************************************
* m3dQUAT_DUAL::DLB()
* DLB: linear blend (without normalization) of N dual quaternions with N convex weights
*************************************************************/
void m3dQUAT_DUAL::DLB (int N, const float weight[], const m3dQUAT_DUAL q[], m3dQUAT_DUAL * __RESTRICT res)
{
   ASSERT(N > 0);
   m3dScaleQuatDual(&q[0], weight[0], res);

   for (int n = 1; n < N; n++) {
      m3dQUAT_DUAL scale;

      m3dScaleQuatDual(&q[n], weight[n], &scale); 
      m3dAddQuatDual(res, &scale, res);
   }
}

/*************************************************************
* m3dAddQuatDual()
* 
*************************************************************/
void m3dAddQuatDual (const m3dQUAT_DUAL * __RESTRICT a, const m3dQUAT_DUAL * __RESTRICT b, m3dQUAT_DUAL * __RESTRICT res)
{
   m3dAddQuat(&a->q0, &b->q0, &res->q0);
   m3dAddQuat(&a->qe, &b->qe, &res->qe);
}

/*************************************************************
* m3dSubtractQuatDual()
* 
*************************************************************/
void m3dSubtractQuatDual (const m3dQUAT_DUAL * __RESTRICT a, const m3dQUAT_DUAL * __RESTRICT b, m3dQUAT_DUAL * __RESTRICT res)
{
   m3dSubtractQuat(&a->q0, &b->q0, &res->q0);
   m3dSubtractQuat(&a->qe, &b->qe, &res->qe);
}

/*************************************************************
* m3dScaleQuatDual()
* 
*************************************************************/
void m3dScaleQuatDual (const m3dQUAT_DUAL * __RESTRICT a, float scale, m3dQUAT_DUAL * __RESTRICT res)
{
   m3dScaleQuat(&a->q0, scale, &res->q0);
   m3dScaleQuat(&a->qe, scale, &res->qe);
}

/*************************************************************
* m3dMultiplyQuatDual()
* 
*************************************************************/
void m3dMultiplyQuatDual (const m3dQUAT_DUAL * __RESTRICT a, const m3dQUAT_DUAL * __RESTRICT b, m3dQUAT_DUAL * __RESTRICT resPtr)
{
   m3dQUAT_DUAL res;

   m3dMultiplyQuat(&a->q0, &b->q0, &res.q0);

   m3dQUAT q2;
   m3dMultiplyQuat(&a->q0, &b->qe, &q2);
   m3dMultiplyQuat(&a->qe, &b->q0, &res.qe);
   m3dAddQuat(&res.qe, &q2, &res.qe);
   *resPtr = res;
}

/*************************************************************
* m3dIsEqualQuatDual_A()
* 
*************************************************************/
BOOL m3dIsEqualQuatDual_A (const m3dQUAT_DUAL * __RESTRICT a, const m3dQUAT_DUAL * __RESTRICT b, float acc)
{
   return m3dIsEqualQuat_A(&a->q0, &b->q0, acc) && m3dIsEqualQuat_A(&a->qe, &b->qe, acc);
}

/*************************************************************
* m3dLineCombine()
*************************************************************/
void m3dLineCombine (const m3dQUAT_DUAL * __RESTRICT a, const m3dQUAT_DUAL * __RESTRICT b, float kb, m3dQUAT_DUAL * __RESTRICT res)
{
   res->q0.s   = a->q0.s +   b->q0.s * kb;
   res->q0.v.x = a->q0.v.x + b->q0.v.x * kb;
   res->q0.v.y = a->q0.v.y + b->q0.v.y * kb;
   res->q0.v.z = a->q0.v.z + b->q0.v.z * kb;

   res->qe.s   = a->qe.s +   b->qe.s * kb;
   res->qe.v.x = a->qe.v.x + b->qe.v.x * kb;
   res->qe.v.y = a->qe.v.y + b->qe.v.y * kb;
   res->qe.v.z = a->qe.v.z + b->qe.v.z * kb;
}

//
// End-of-file M3D_DUALQUAT.CPP
//
